package org.pi.common.vision.tool

import org.pi.common.matrix.FloatMatrix

trait ColorSpaceToolTrait {

  def rgb2hsv(rgb: FloatMatrix) : FloatMatrix = {

    def rgb2hsvFromImage(rgb: FloatMatrix) : FloatMatrix = {
      val hsv = FloatMatrix( rgb.shape : _* )
      val h = rgb.length(0)
      val w = rgb.length(1)
      var index = 0
      while (index < w*h) {

    	val maxValue = Math.max(Math.max(rgb(index+0), rgb(index+w*h)),
                             rgb(index+2*w*h));
        val minValue = Math.min(Math.min(rgb(index+0), rgb(index+w*h)),
                             rgb(index+2*w*h));

        if (maxValue - minValue != 0) {
          if (rgb(index+0) == maxValue) {
            hsv(index+0) = (60.0f *
                ( (rgb(index+w*h) - rgb(index+2*w*h)) / (maxValue - minValue))) / 360.0f;
          }
          else if (rgb(index+w*h) == maxValue) {
            hsv(index+0) = (60.0f *
                ( (rgb(index+2*w*h) - rgb(index+0)) / (maxValue - minValue)) +
                120.0f) / 360.0f;
          }
          else {
            hsv(index+0) = (60.0f *
                ( (rgb(index+0) - rgb(index+w*h)) / (maxValue - minValue)) +
                240.0f) / 360.0f;
          }
        }
        else {
          hsv(index+0) = .0f;
        }
        if (maxValue != 0) {
          hsv(index+w*h) = (maxValue - minValue) / maxValue;
        }
        else {
          hsv(index+w*h) = .0f;
        }
        hsv(index+2*w*h) = maxValue;
        
        index += 1
      } // end for x,y
      hsv
    }

    if (rgb.shape.length==3 && rgb.length(2)==3) {
      rgb2hsvFromImage(rgb)
    }
    else if (rgb.shape.length==2 && rgb.length(1)==3) {
      val h = rgb.length(0)
      rgb2hsvFromImage(rgb.reshape(h,1,3)).reshape(h,3)
    }
    else throw new Error("unsupported matrix shape "+rgb.shape)
        
  }

  def hsv2rgb(hsv: FloatMatrix) : FloatMatrix = {

    def hsv2rgbFromImage(hsv: FloatMatrix) : FloatMatrix = {
      def bool2value(value:Boolean) : Float = {
        if (value) 1
        else 0
      }
      val rgb = FloatMatrix( hsv.shape : _* )
      val h = hsv.length(0)
      val w = hsv.length(1)
      var index = 0
      while(index<w*h) {
        val hue = hsv(index+0)
        val s = hsv(index+w*h)
        val i = hsv(index+2*w*h)
        val r1 = hue-2.0f/3.0f-Math.floor(hue-2.0f/3.0f).toFloat;
        val g1 = hue;
        val b1 = hue-1.0f/3.0f-Math.floor(hue-1.0f/3.0f).toFloat;
        val f = s*i;
        rgb(index+0) = i*(1-s) + f*(6.0f*(bool2value(r1<1.0f/6.0f))*r1 + (bool2value(r1>=1.0f/6.0f && r1<0.5f)) + (bool2value(r1>=0.5f && r1<2.0f/3.0f))*(4-6*r1));
        rgb(index+w*h) = i*(1-s) + f*(6.0f*(bool2value(g1<1.0f/6.0f))*g1 + (bool2value(g1>=1.0f/6.0f && g1<0.5f)) + (bool2value(g1>=0.5f && g1<2.0f/3.0f))*(4-6*g1));
        rgb(index+2*w*h) = i*(1-s) + f*(6.0f*(bool2value(b1<1.0f/6.0f))*b1 + (bool2value(b1>=1.0f/6.0f && b1<0.5f)) + (bool2value(b1>=0.5f && b1<2.0f/3.0f))*(4-6*b1));
        
        index+=1
      }
      rgb
    }

    if (hsv.shape.length==3 && hsv.length(2)==3) {
      hsv2rgbFromImage(hsv)
    }
    else if (hsv.shape.length==2 && hsv.length(1)==3) {
      val h = hsv.length(0)
      hsv2rgbFromImage(hsv.reshape(h,1,3)).reshape(h,3)
    }
    else throw new Error("unsupported matrix shape "+hsv.shape)
  }
}

object ColorSpaceTool extends ColorSpaceToolTrait {
	def main(args: Array[String]) : Unit = {
		println("start ColorSpaceTool")
		
		import org.pi.common.vision.Api.imread
		import org.pi.common.vision.Api.imwrite
		import org.pi.common.matrix.range.Api._
		import org.pi.common.time.Api._
		
		val a = imread("data/4Spiele.png")
		
		tic
		val b = rgb2hsv(a)
		toc("rgb2hsv(a)")
		
		tic
		b(~~,~~,0) = 0.0f
		toc("b(~~,~~,0) = 0.0f")
		
		tic
		val c = hsv2rgb(b)
		toc("hsv2rgb(b)")
		
		imwrite("output.png",c)
		println("end ColorSpaceTool")
	}
}